import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import numpy as np

print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
                anonymous=True)

robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm_group")

group.set_planner_id("RRTConnectkConfigDefault")

display_trajectory_publisher = rospy.Publisher(
                                    '/move_group/display_planned_path',
                                    moveit_msgs.msg.DisplayTrajectory)

print "============ Reference frame: %s" % group.get_planning_frame()
print "============ Reference frame: %s" % group.get_end_effector_link()

print "============ Robot Groups:"
print robot.get_group_names()
print "============ Printing robot state"
print robot.get_current_state()
print "============"

print "============ Generating plan 1"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = 1.0
pose_target.orientation.y = 0.0
pose_target.orientation.z = 0.0
pose_target.orientation.w = 0.0
pose_target.position.x = 0.6
pose_target.position.y = 0.0
pose_target.position.z = 1.2
group.set_pose_target(pose_target)

plan1 = group.plan()

print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(0.5)

print "============ Visualizing plan1"
display_trajectory = moveit_msgs.msg.DisplayTrajectory()

display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory);

print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(1.5)

# Uncomment below line when working with a real robot
group.execute(plan1)

print "============ Waiting while plan1 is executed..."
rospy.sleep(1.5)

waypoints = []
PI = 3.14;
x_range = 0.15

waypoints.append(group.get_current_pose().pose)
y_lower_bound = -0.4
y_upper_bound = 0.4

steps = 20
num_success = 0
for i in range(0,steps+1):
	temp_pose_target = geometry_msgs.msg.Pose()
	theta  = i*2*PI/100
	temp_pose_target.position.x = x_range#radius*np.cos(theta)
	temp_pose_target.position.y = ( i*(y_upper_bound-y_lower_bound)/steps ) + y_lower_bound
	temp_pose_target.position.z = 1.0
	temp_pose_target.orientation.x = 1
	temp_pose_target.orientation.y = 0
	temp_pose_target.orientation.z = 0
	temp_pose_target.orientation.w = 0
	group.set_pose_target(temp_pose_target)
	if(group.plan()):
		plan3 = group.plan()
		# num_success = num_success+1
		print "============ Waiting while RVIZ displays plan3..."
		rospy.sleep(0.5)
		if(group.execute(plan3)):
			rospy.sleep(1.5)
			num_success = num_success+1
print "success rate: "
print num_success/steps



moveit_commander.roscpp_shutdown()